import com.ridgesoft.intellibrain.IntelliBrain;
import com.ridgesoft.intellibrain.IntelliBrainDigitalIO;
import com.ridgesoft.robotics.Servo;
import com.ridgesoft.robotics.RangeFinder;
import com.ridgesoft.robotics.sensors.ParallaxPing;
import com.ridgesoft.io.Display;

public class hallMonitor {
	private static int KILLPORT = 3;
	private static int MOTOPORT = 1;
	private static int STEERPORT = 2;
	private static int LEFTUSPORT = 4;
	private static int RIGHTUSPORT = 5;
	
	//movement
	private static int LTURN = 25;
	private static int RTURN = 100;
	private static int GOFWD = 65;
	static int value;

    public static void main(String args[]) {
		//setup everything
		Servo moto = IntelliBrain.getServo(MOTOPORT);
        IntelliBrainDigitalIO killMe = IntelliBrain.getDigitalIO(KILLPORT);		
		Display display = IntelliBrain.getLcdDisplay();
        RangeFinder leftUS = new ParallaxPing(IntelliBrain.getDigitalIO(LEFTUSPORT));
        RangeFinder rightUS = new ParallaxPing(IntelliBrain.getDigitalIO(RIGHTUSPORT));
		Servo steer = IntelliBrain.getServo(STEERPORT);

		//setup kill pulse, motor
            try{
				killMe.enablePulseMeasurement(true);
				moto.setPosition(50);
				display.print(0,"Neutral"); 
				Thread.sleep(3000);
			} catch(Throwable t){
				t.printStackTrace();
			}
			
		while(true){

        try {    
                rightUS.ping();
                long timeout = System.currentTimeMillis() + 20;
                float distance;
                do {
                    distance = rightUS.getDistanceInches();
					if(distance < 40  && distance > 0){
						display.print(0, "TURN RIGHT GUY!");
						steer.setPosition(LTURN);
						try{
							Thread.sleep(500);
						} catch (Throwable t){
							t.printStackTrace();
						} finally {
							steer.setPosition(GOFWD);
						}
					} 					
					
                } while ((distance < 0.0f)
                        && (System.currentTimeMillis() < timeout));
				
                leftUS.ping();
                long timeoutL = System.currentTimeMillis() + 20;
                float distanceL;
                do {
                    distanceL = leftUS.getDistanceInches();
					
					if(distanceL < 40  && distanceL > 0){
						if(distance < 40 && distance > 0){
							while(true){
							moto.setPosition(50);
							moto.off();
							display.print(0, "STOP NOW");
							}
						} else {
							steer.setPosition(RTURN);
							try{
								Thread.sleep(500);
							} catch (Throwable t){
								t.printStackTrace();
							} finally {
								steer.setPosition(GOFWD);
							}
						}
					} 					
                } while ((distanceL < 0.0f)
                        && (System.currentTimeMillis() < timeoutL));
				display.print(1, distanceL+"");
				
				// if(distance < 30){
					// moto.setPosition(LTURN);
					// try{
						// Thread.sleep(500);
					// } catch (Throwable t){
						// t.printStackTrace();
					// }
				// } 
		
			
		steer.setPosition(GOFWD); //set straight
		moto.setPosition (60);	
		//Thread.sleep(1000);		
					//kill it
			if(killMe.isSet() && (killMe.readPulseDuration() > 1000)){
				display.print(0, "RAMrod down. :(");
				moto.off();
				steer.off();
				Thread.sleep(2000);
				break;
			}  
        }
        catch (Throwable t) {
            t.printStackTrace();
        }}
    }
}